/*
 * Copyright (c) Huawei Technologies Co., Ltd. 2020-2021.
 * Description: support fiq-gicv3
 * Author: yanbo <joey.yanbo@huawei.com>
 * Create: 2020-05-05
 */

#include "../hal/fiq_glue_com/fiq_glue_private.h"

#define ICC_EOIR0	__ACCESS_CP15(c12, 0, c8, 1)
#define ICC_IAR0	__ACCESS_CP15(c12, 0, c8, 0)
#if defined(CONFIG_RTOS_HAL_SUPPORT_GICV3_FOR_AARCH32) && defined(CONFIG_RTOS_HAL_IPI_COMBINE)
#define ICC_SGI0R       __ACCESS_CP15_64(2, c12)
#define ICC_SGI0R_IRM_BIT 40
#endif

static DEFINE_SPINLOCK(fiq_set_lock);

static void gicv3_clear_pending(struct irq_data *d)
{
	void __iomem *reg;
	u32 mask;

	reg = gic_dist_base(d) + GICD_ICPENDR + (gic_irq(d) / 32) * 4;
	mask = 1 << (gic_irq(d) % 32);
	writel_relaxed(mask, reg);
}

static void gicv3_set_priority(struct irq_data *d, unsigned int priority)
{
	void __iomem *reg;
	u32 shift, val, mask;

	reg = gic_dist_base(d) + GICD_IPRIORITYR + (gic_irq(d) / 4 * 4);
	shift = (gic_irq(d) % 4) * 8;
	mask = 0xff << shift;

	if (priority > 0xff)
		priority = 0xff;

	val = readl_relaxed(reg);
	val &= ~mask;
	val |= priority << shift;
	writel_relaxed(val, reg);
}

static void gicv3_set_group0(struct irq_data *d)
{
	void __iomem *reg_igroup, *reg_igrpmodr;
	u32 shift, val_igroup, val_igrpmodr, mask, fiq;

	fiq = gic_irq(d);

	reg_igroup = gic_dist_base(d) + GICD_IGROUPR + (fiq / 32) * 4;
	reg_igrpmodr = gic_dist_base(d) + GICD_IGRPMODR + (fiq / 32) * 4;

	shift = fiq % 32;
	mask = 1 << shift;

	val_igroup = readl_relaxed(reg_igroup);
	val_igrpmodr = readl_relaxed(reg_igrpmodr);

	val_igroup &= ~mask;
	val_igrpmodr &= ~mask;

	writel_relaxed(val_igroup, reg_igroup);
	writel_relaxed(val_igrpmodr, reg_igrpmodr);
}

static inline u32 gic_read_iar0(void)
{
	u32 irqstat = read_sysreg(ICC_IAR0);

	dsb(sy);
	return irqstat;
}

static int gicv3_get_fiqnum(void)
{
	u32 irqnr;

	irqnr = gic_read_iar0();
	return irqnr;
}

static inline void gic_write_eoir0(u32 irq)
{
	write_sysreg(irq, ICC_EOIR0);
	isb();
}

static void gicv3_eoi_fiq(unsigned int fiq)
{
	gic_write_eoir0(fiq);
}

static int gicv3_set_fiq(unsigned int fiq, unsigned int priority)
{
	struct irq_desc *desc = NULL;
	struct irq_data *data = NULL;
	unsigned long flags;

	desc = irq_to_desc(fiq);
	data = irq_desc_get_irq_data(desc);
	if (!data->chip_data) {
		/*
		 * if the irq  is ipi, chip_data is NULL.
		 * we can't use this function to set it.
		 */
		pr_err("fiq%d chip data is NULL.\n", fiq);
		return -EINVAL;
	}

	/* set fiq to gic security group0 */
	spin_lock_irqsave(&fiq_set_lock, flags);
	gic_mask_irq(data);
	gicv3_clear_pending(data);
	gicv3_set_group0(data);
	gicv3_set_priority(data, priority);
	gic_unmask_irq(data);
	spin_unlock_irqrestore(&fiq_set_lock, flags);
	return 0;
}


static void gicv3_raise_softfiq(void)
{
#if defined(CONFIG_RTOS_HAL_SUPPORT_GICV3_FOR_AARCH32) && defined(CONFIG_RTOS_HAL_IPI_COMBINE)
	if (ipi0_to_fiq)
		/* send ipi0 to other all cpus */
		write_sysreg(1ULL << ICC_SGI0R_IRM_BIT, ICC_SGI0R);
#endif
}

static void gicv3_fiq_init(void)
{
	struct fiq_gic_handle fiq_handle_v3 = {
		.set_fiq	= gicv3_set_fiq,
		.raise_softfiq	= gicv3_raise_softfiq,
		.get_fiq_num	= gicv3_get_fiqnum,
		.eoi_fiq	= gicv3_eoi_fiq,
	};

	fiq_register_gic(&fiq_handle_v3);
}
